{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "dede2334",
   "metadata": {
    "colab_type": "text",
    "id": "EgiF12Hf1Dhs"
   },
   "source": [
    "# **Input and Output Ports of the Hardware Station**\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "8e577e50",
   "metadata": {
    "lines_to_end_of_cell_marker": 2
   },
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from pydrake.all import StartMeshcat, namedview\n",
    "\n",
    "from manipulation.exercises.grader import Grader\n",
    "from manipulation.exercises.robot.test_hardware_station_io import (\n",
    "    TestHardwareStationIO,\n",
    ")\n",
    "from manipulation.station import MakeHardwareStation, load_scenario\n",
    "from manipulation.utils import RenderDiagram"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "fdc77929",
   "metadata": {},
   "outputs": [],
   "source": [
    "meshcat = StartMeshcat()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "07d0a868",
   "metadata": {
    "colab_type": "text",
    "id": "bClJCCHTKBb5"
   },
   "source": [
    "## Access System Input/Output Values\n",
    "In this exercise, you will explore the [`HardwareStation`](http://manipulation.mit.edu/robot.html#hardware_station) that was mentioned during the lecture. You should recall that the orange ports are the ones that do not exist for the actual hardware platform.\n",
    "\n",
    "<html>\n",
    "<table align=\"center\" cellpadding=\"0\" cellspacing=\"0\"><tbody><tr align=\"center\"><td style=\"vertical-align:middle\"><table cellspacing=\"0\" cellpadding=\"0\"><tbody><tr><td align=\"right\" style=\"padding:5px 0px 5px 0px\">iiwa.position\u2192</td></tr><tr><td align=\"right\" style=\"padding:5px 0px 5px 0px\">iiwa.feedforward_torque (optional)\u2192</td></tr><tr><td align=\"right\" style=\"padding:5px 0px 5px 0px\">wsg.position\u2192</td></tr><tr><td align=\"right\" style=\"padding:5px 0px 5px 0px\">wsg.force_limit (optional)\u2192</td></tr></tbody></table></td><td align=\"center\" style=\"border:solid;padding-left:20px;padding-right:20px;vertical-align:middle\" bgcolor=\"#F0F0F0\"><a href=\"https://github.com/RussTedrake/manipulation/blob/ceb817b527cbf1826c5b9a573ffbef415cb0f013/manipulation/scenarios.py#L453\">HardwareStation</a></td><td style=\"vertical-align:middle\"><table cellspacing=\"0\" cellpadding=\"0\"><tbody><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 iiwa.position_commanded</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 iiwa.position_measured</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 iiwa.velocity_estimated</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 iiwa.state_estimated</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 iiwa.torque_commanded</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 iiwa.torque_measured</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 iiwa.torque_external</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 wsg.state_measured</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 wsg.force_measured</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 camera_[NAME].rgb_image</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 camera_[NAME].depth_image</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 <b style=\"color:orange\">camera_[NAME].label_image</b></td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 ...</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 camera_[NAME].rgb_image</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 camera_[NAME].depth_image</td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 <b style=\"color:orange\">camera_[NAME].label_image</b></td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 <b style=\"color:orange\">query_object</b></td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 <b style=\"color:orange\">contact_results</b></td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 <b style=\"color:orange\">plant_continuous_state</b></td></tr><tr><td align=\"left\" style=\"padding:5px 0px 5px 0px\">\u2192 <b style=\"color:orange\">body_poses</b></td></tr></tbody></table></td></tr></tbody></table>\n",
    "</html>\n",
    "\n",
    "Now we construct a `HardwareStation` object and finalize the system setting. To get a sense of what this hardware station scenario looks like, you can open the meshcat viewer from the generated link as usual. There should be a bookshelf and a Kuka arm with a gripper attached (it might take a few seconds to load)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "99b20184",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "SN1HkYCy8VJo"
   },
   "outputs": [],
   "source": [
    "scenario_data = \"\"\"\n",
    "directives:\n",
    "- add_directives:\n",
    "    file: package://manipulation/clutter.dmd.yaml\n",
    "model_drivers:\n",
    "    iiwa: !IiwaDriver\n",
    "      hand_model_name: wsg\n",
    "    wsg: !SchunkWsgDriver {}\n",
    "\"\"\"\n",
    "scenario = load_scenario(data=scenario_data)\n",
    "station = MakeHardwareStation(scenario, meshcat=meshcat)\n",
    "context = station.CreateDefaultContext()\n",
    "\n",
    "station.GetInputPort(\"wsg.position\").FixValue(context, [0.1])\n",
    "station.ForcedPublish(context)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "ac4346da",
   "metadata": {
    "colab_type": "text",
    "id": "82VsgQ938k7v"
   },
   "source": [
    "[**Context**](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=context#pydrake.systems.framework.Context_) is an abstract class template that represents all the typed values that are used in a System\u2019s computations: time, numeric-valued input ports, numerical state, and numerical parameters. There are also type-erased abstract state variables, abstract-valued input ports, abstract parameters, and a double accuracy setting. It is important to note that a **Context** is designed to be used only with the System that created it. State and Parameter data can be copied between contexts for compatible systems as necessary. One of the most common mistakes is to pass the wrong context. Most methods in Drake should throw an error if you pass a context from the wrong system, but a few do not perform this check yet.\n",
    "\n",
    "In the cell below, we first create a root context from the diagram, and then we retrieve the contexts of the subsystems from the root context."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "eed91de3",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "BS7uvKcIjwp0"
   },
   "outputs": [],
   "source": [
    "# initialize context\n",
    "station_context = station.CreateDefaultContext()\n",
    "plant = station.GetSubsystemByName(\"plant\")\n",
    "plant_context = plant.GetMyContextFromRoot(station_context)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "02d5daab",
   "metadata": {
    "colab_type": "text",
    "id": "_AuF4TWGlbFp"
   },
   "source": [
    "In this exercise, you will familiarize yourself with the input and output mechanism from the hardware station system. Remember you can always generate a schematic view of your system by running the cell below. By clicking the \"+\" sign on the manipulation_station, you can get a more detailed view of the diverse modules within the hardware station. (You might need to run the cell twice to see the diagram)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "7f13cbcc",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "OvSvFZH4_9Ni"
   },
   "outputs": [],
   "source": [
    "RenderDiagram(station, max_depth=1)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a240e4f1",
   "metadata": {
    "colab_type": "text",
    "id": "FKDGlrCFqb15"
   },
   "source": [
    "Now if we set the joint position of the Kuka arm, we should expect to get the same values from the `iiwa.position_measured port`, which can be found from the output ports of `station` object. Below we demonstrate how this can be done using Drake's syntax. You may also find it useful to review the `System` class documentation [here](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=output_port#pydrake.systems.framework.System_)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "68d04d72",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "PwWvrx1_cPLl"
   },
   "outputs": [],
   "source": [
    "# provide initial states\n",
    "q0 = np.array([-1.57, 0.1, 0, -1.2, 0, 1.6, 0])\n",
    "iiwa = plant.GetModelInstanceByName(\"iiwa\")\n",
    "# set the joint positions of the kuka arm\n",
    "plant.SetPositions(plant_context, iiwa, q0)\n",
    "# examine the output port\n",
    "station.GetOutputPort(\"iiwa.position_measured\").Eval(station_context)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "bdb990c5",
   "metadata": {},
   "source": [
    "Note that the [output port](https://drake.mit.edu/pydrake/pydrake.systems.framework.html?highlight=outputport#pydrake.systems.framework.OutputPort) named `iiwa.position_measured` is first retrieved from the station and then evaluated using **Eval** method. This is a very common approach to read the values of a selected output port.\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7515a1b3",
   "metadata": {},
   "source": [
    "*Drake advice:* use `nameview` to refer to positions by name."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6e594441",
   "metadata": {},
   "outputs": [],
   "source": [
    "IiwaPositionView = namedview(\n",
    "    \"IiwaPositions\",\n",
    "    plant.GetPositionNames(iiwa, always_add_suffix=False),\n",
    ")\n",
    "q0 = IiwaPositionView.Zero()\n",
    "q0.iiwa_joint_1 = -1.57\n",
    "q0.iiwa_joint_2 = 0.1\n",
    "q0.iiwa_joint_4 = -1.2\n",
    "q0.iiwa_joint_6 = 1.6\n",
    "print(q0)\n",
    "plant.SetPositions(plant_context, iiwa, q0[:])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "fc8317c0",
   "metadata": {
    "colab_type": "text",
    "id": "8d7sH3kzrnYL"
   },
   "source": [
    "Sometimes, when working directly with a `MultibodyPlant`, you may retrieve the joint angles directly from the **plant**. But this will not work when we're running on real hardware. Use the input and output ports to properly leverage the \"sim 2 real\" abstraction."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "b7bc3262",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "gbpZuVa_axTY"
   },
   "outputs": [],
   "source": [
    "print(plant.GetPositions(plant_context, iiwa))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f8e4a9a6",
   "metadata": {
    "colab_type": "text",
    "id": "Ey-t3M_Usun3"
   },
   "source": [
    "# Exercise a: Code Submission\n",
    "Now, it's your time to code! Use **GetOutputPort** and **Eval** to retrieve the joint velocities from the \"iiwa_velocity_estimated\" output port. Note that we have set the velocities for you."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "6ec35164",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "A34DVaMRcVKV"
   },
   "outputs": [],
   "source": [
    "plant.SetVelocities(\n",
    "    plant_context,\n",
    "    iiwa,\n",
    "    np.zeros(\n",
    "        7,\n",
    "    ),\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "f1646337",
   "metadata": {
    "colab_type": "text",
    "id": "449kizGfUuwR"
   },
   "source": [
    "Below, `get_velocity(station, station_context)` is the function you must modify to query values from \"iiwa_velocity_estimated\"."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "040cde6e",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "RWa-mQwKRG4z"
   },
   "outputs": [],
   "source": [
    "def get_velocity(station, station_context):\n",
    "    \"\"\"\n",
    "    fill in your code in this method\n",
    "    \"\"\"\n",
    "    velocity_estimated = None\n",
    "    return velocity_estimated"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "69109244",
   "metadata": {
    "colab_type": "text",
    "id": "X8FLrTP-U3bH"
   },
   "source": [
    "You can check if you got the implementation correct by running the below autograder."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "5ab19732",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "fVShh5kyRG44"
   },
   "outputs": [],
   "source": [
    "Grader.grade_output([TestHardwareStationIO], [locals()], \"results.json\")\n",
    "Grader.print_test_results(\"results.json\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "e128f69c",
   "metadata": {
    "colab_type": "text",
    "id": "EEbQAd8SuvzQ"
   },
   "source": [
    "Please note that the `iiwa.position_commanded` and the `iiwa.position_measured` are NOT the same variable. The `iiwa.position_commanded` are the commanded positions sent to the robot, whereas the `iiwa.positions` are the current positions of the simulated robot. We also expect to have different values for the feed-forward torque (system input) and the commanded torque (system output). Next, you will investigate why. First, let us provide a zero feed-forward torque to the `iiwa.feedforward_torque` port."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "f0dedc90",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "gDV6v7YMlKzl"
   },
   "outputs": [],
   "source": [
    "station.GetInputPort(\"iiwa.feedforward_torque\").FixValue(\n",
    "    station_context,\n",
    "    np.zeros(\n",
    "        7,\n",
    "    ),\n",
    ")\n",
    "tau_no_ff = station.GetOutputPort(\"iiwa.torque_commanded\").Eval(\n",
    "    station_context\n",
    ")\n",
    "print(\n",
    "    \"feedforward torque: {}\".format(\n",
    "        np.zeros(\n",
    "            7,\n",
    "        )\n",
    "    )\n",
    ")\n",
    "print(\"commanded torque with no feedforward torque:{}\".format(tau_no_ff))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "58306c05",
   "metadata": {
    "colab_type": "text",
    "id": "WDP0uoxtvtrq"
   },
   "source": [
    "Now try the same experiment with a non-zero feedforward torque as shown below."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "07151f24",
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "zRxNOJKTfDbT"
   },
   "outputs": [],
   "source": [
    "tau_ff = np.linspace(3.1, 3.7, 7)\n",
    "print(\"feedforward torque: {}\".format(tau_ff))\n",
    "station.GetInputPort(\"iiwa.feedforward_torque\").FixValue(\n",
    "    station_context, tau_ff\n",
    ")\n",
    "torque_commanded = station.GetOutputPort(\"iiwa.torque_commanded\").Eval(\n",
    "    station_context\n",
    ")\n",
    "print(\"the commanded torque: {}\".format(torque_commanded))"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "a2e78e9b",
   "metadata": {
    "colab_type": "text",
    "id": "WKh2ETazw5-W"
   },
   "source": [
    "# Exercise b: Written Problem.\n",
    "Below, we have a question for you.\n",
    "\n",
    "**In this exercise, please explain what causes the discrepancy between the feedforward torque and the commanded torque.**\n",
    "\n",
    "HINT: can you find any relationship among *tau_ff*, *tau_no_ff*, *torque_commanded*?"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "d3758425",
   "metadata": {
    "colab_type": "text",
    "id": "U2cjfMsITgVF"
   },
   "source": [
    "## Your Answer\n",
    "\n",
    "Answer the Question here, and copy-paste to the Gradescope 'written submission' section!\n"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "554389c2",
   "metadata": {
    "colab_type": "text",
    "id": "SCvUhPHMTqiH"
   },
   "source": [
    "## How will this notebook be graded?"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "366dc36e",
   "metadata": {
    "colab_type": "text",
    "id": "l5OAsgvJTt1M"
   },
   "source": [
    "We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:\n",
    "- [5pts] `get_velocity` must be implemented correctly.\n",
    "- [5pts] You must answer correctly why there is a difference between feed-forward torque and commanded torque."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "81456347",
   "metadata": {
    "colab_type": "text",
    "id": "w48SCV0HUbcJ"
   },
   "source": [
    "## Additional Note."
   ]
  },
  {
   "cell_type": "markdown",
   "id": "3dc43869",
   "metadata": {
    "colab_type": "text",
    "id": "R324XG0CvLiN"
   },
   "source": [
    "So far we have not looked into the outputs of simulated camera sensors. Fortunately, accessing the camera data can be done in an almost exactly the same way as we have shown above. We will get to it soon!"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.10.6 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.11.6"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}